class PidController:
    # 给pid的三个参数赋初值
    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.iLimit= 200 # 积分上限

        self.last_error = 0.0
        self.integral = 0.0
        

    def changePidParams(self, kp, ki, kd, iLimit):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.iLimit = iLimit

    def pidUpdate(self, error, dt):

        p = self.kp * error

        self.integral += error

        if(self.integral > self.iLimit):
            self.integral = self.iLimit
        elif(self.integral < -self.iLimit):
            self.integral = -self.iLimit
            
        i = self.ki * self.integral

        derivative = (error - self.last_error) / dt
        d = self.kd * derivative
        self.last_error = error

        return p + i + d

